/**
 * @file utils
 * @brief This file contains all the functions prototypes for the utils
 *        firmware library.
 * @author He Shaohua (hsh15529870639@foxmail.com)
 * @date 2024/11/13
 * @version 1.0
 * @copyright Copyright (c) 2024, SNUT-Chaoyue Team
 * ****************************************************************************************************
 * @par Change Log:
 * <table>
 * <tr><th>Date         <th>Version       <th>Author      <th>Description
 * <tr><td>2024/11/13      <td> 1.0          <td>He Shaohua  <td>Creator
 *
 * </table>
 * ==================================----How to use this
 * driver----=====================================
 * [..]
 * @note
 *   -#
 *
 * @warning
 *   -#
 *
 * *****************************************************************************************************
 * @attention
 *
 * This document is required and please understand any bugs that may occur in
 * the document. If you want to modify this file, after the code update is
 * completed, update the version number in time, indicate the update date, and
 * fill in the changes Make sure that the submitted code does not have many
 * bugs.
 *
 * All rights reserved.
 * *****************************************************************************************************
 */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef MOTOR_DRIVER_USER_UTILS_H_
#define MOTOR_DRIVER_USER_UTILS_H_
/* Includes ------------------------------------------------------------------*/

/* Private macros ------------------------------------------------------------*/
/* Private type --------------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported variables --------------------------------------------------------*/
constexpr float  MY_PI = 3.14159265354f;
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
#include "fdcan.h"
#include "cmsis_os2.h"
#include "gpio.h"
#include "stm32g4xx_hal.h"
#include "adc.h"
#include "tim.h"
#include "usart.h"
/**
 * @brief Vofa可变长浮点数输出
 * @param args 参数包
 */
template <typename... T>
void Debug_Send(T... args)
{
    static uint8_t temp[40] = {0};
    uint8_t k = 0;
    // 数据
    ((
         [&](T arg)
         {
             const auto* bytePtr = reinterpret_cast<uint8_t*>(&arg);
             temp[k++] = bytePtr[0];
             temp[k++] = bytePtr[1];
             temp[k++] = bytePtr[2];
             temp[k++] = bytePtr[3];
         }(args)),
     ...);
    // 帧尾
    temp[k++] = 0x00;
    temp[k++] = 0x00;
    temp[k++] = 0x80;
    temp[k++] = 0x7f;
    HAL_UART_Transmit_DMA(&huart2, temp, k);
}

/**
 * @brief 绝对值函数
 * @param x 输入值
 * @attention None
 */
template <typename Type>
Type my_Abs(Type x)
{
    return ((x > 0) ? x : -x);
}

/**
 * @brief 限幅函数
 * @param x 输入值地址
 * @param Min 最小范围
 * @param Max 最大范围
 * @attention None
 */
template <typename Type>
void my_Constrain(Type* x, Type Min, Type Max)
{
    if (*x < Min)
        *x = Min;
    if (*x > Max)
        *x = Max;
}

void can_init(uint32_t rx_id);
void can_send(uint16_t tx_id, const uint8_t* tx_buffer);

void Flash_Page1_Read(uint32_t *dataarr);
void Flash_Page1_Save(uint32_t *dataarr);
/**
 * PI控制器
 */
class PI_Controller
{
public:
    /// PI控制器初始化
    /// @param _kp 比例项
    /// @param _ki 积分项
    /// @param _constrain 限幅为-_constrain --- +_constrain
    explicit PI_Controller(const float _kp, const float _ki, const float _constrain) :
        kp(_kp), ki(_ki), constrain(_constrain)
    {
    }

    /// PI运行函数
    /// @param now 设置当前值
    /// @param target 设置目标值
    /// @return 计算出的值
    [[nodiscard]] float operator()(const float now, const float target)
    {
        float delta = 0.0f;
        const float error = target - now;
        delta = error * kp;
        Integral += error * ki;
        my_Constrain<float>(&Integral, -constrain, constrain);
        return delta + Integral;
    }

    /// 比例项设置
    /// @param _kp 比例因子
    void set_kp(const float _kp) { kp = _kp; }

    /// 积分项设置
    /// @param _ki 积分项常数
    void set_ki(const float _ki) { ki = _ki; }

    /// 复位PI控制器
    void reset() { Integral = 0.0f; };

private:
    float kp = 0.0f;
    float ki = 0.0f;
    float constrain = 0.0f;
    float Integral = 0.0f;
};

/**
   * @brief 一阶卡尔曼滤波
   *
 */
class Kalman_FristOrder
{
public:
    /// 卡尔曼滤波器的初始化
    /// @param T_Q 过程噪声
    /// @param T_R 测量噪声
    /// @note Q值为过程噪声，越小系统越容易收敛，表示对模型预测的值信任度越高,R值为测量噪声。
    ///       R太大，滤波的响应（此处响应特指对量测值的响应）会变慢，因为它对新测量的值的信任度降低
    explicit Kalman_FristOrder(float T_Q, float T_R) : Q(T_Q), R(T_R){};
    float operator()(float data)
    {
        this->X_mid = this->X_last;
        this->P_mid = this->P_last + this->Q;
        this->Kg = this->P_mid / (this->P_mid + this->R);
        this->X_last = this->X_mid + this->Kg * (data - this->X_mid);
        this->P_last = (1.0f - this->Kg) * this->P_mid;
        return this->X_last;
    };

private:
    float X_last = 0.0f, X_mid = 0.0f;
    float P_mid = 0.0f, P_last = 0.0f;
    float Kg = 0.0f;
    float Q = 0.0f;
    float R = 0.0f;
};

union u_tag
{
    float f;
    uint32_t i;
};
uint32_t Float_HEX(float fdata);
float HEX_float(uint32_t data);
#endif /* MOTOR_DRIVER_USER_UTILS_H_ */
/**
 * @}
 */

/**
 * @}
 */

/**
 * @}
 */
/*********************************END OF FILE*********************************/
